#include "correspond.h"
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/features/normal_3d.h>
#include "functions.h"

void findCorrespondencesFPFH (
        PointCloud<FPFHSignature33>::Ptr source,
        PointCloud<FPFHSignature33>::Ptr target,
        CorrespondencesPtr correspondences)
{
    cout << "Finding correspondences..." << flush;

    registration::CorrespondenceEstimation<FPFHSignature33,FPFHSignature33> ce;
    ce.setInputCloud(source);
    ce.setInputTarget(target);
    ce.determineReciprocalCorrespondences(*correspondences);//evidentemente non va senza reciprocal
    cout << "OK! Correspondences found: " << correspondences->size() << endl;
}


void filterCorrespondencesSAC ( PointCloud<PointXYZI>::Ptr source_keypoints,
                                PointCloud<PointXYZI>::Ptr target_keypoints,
                                CorrespondencesPtr correspondences,
                                double inlierThreshold)
{
    cout << "Filtering correspondences SAC..." << flush;
    registration::CorrespondenceRejectorSampleConsensus<PointXYZI> rejector;
    rejector.setInputCloud(source_keypoints);
    rejector.setTargetCloud(target_keypoints);
    rejector.setMaxIterations(50);
    rejector.setInlierThreshold(inlierThreshold);
    rejector.setInputCorrespondences(correspondences);
    rejector.getCorrespondences(*correspondences);
    cout << "OK! Correspondences filtered: " << correspondences->size()<< endl;
}
